% Grid Navigation Class
%   Based on psins200324
%   Reference:周琪. (2013). 大飞机全球惯性导航算法研究. 西北工业大学.
%   by Yang Xiaokang  @  NWPU
%   2022-05-18

classdef GridNav < handle
    % GridNav 格网导航类
    
    properties (Access = public)
        nts,                    % 更新周期
        sin_sig, cos_sig,       % 格网角
        qGb,                    % 格网系姿态四元数
        vG,                     % 格网系速度
        pos,                    % 地球系位置
        CGn,                    % 导航坐标系到格网坐标系姿态阵
        CGe,                    % 格网坐标系到地球坐标系姿态阵
        eth,                    % 地球参数结构体
        avp                     % 地理系导航结果
    end
    
    methods (Access = public)
        function obj = GridNav(avp0, nts)
            %GridNav 构造此类的实例
            %   输入trj中的初始信息，采样时间和子样数
            obj.nts = nts;
            obj.avp(7:9) = avp0(7:9);
            obj.eth = earth(obj.avp(7:9));
            [obj.sin_sig,obj.cos_sig] = getGridAngle(obj.avp(7:9));
            obj.CGn = [obj.cos_sig -obj.sin_sig 0; obj.sin_sig obj.cos_sig 0; 0 0 1];
            qnb = a2qua(avp0(1:3));
            obj.qGb = qmul(m2qua(obj.CGn),qnb);
            obj.vG = obj.CGn*avp0(4:6);
            obj.pos = blh2xyz(avp0(7:9));
        end
        
        function obj = update(obj, wm, vm, pos_ref, vn_ref)
            % update 格网导航更新
            %   输入：wm - 角增量(rad)
            %         vm - 速度增量(m/s)
            %         pos_ref - 位置参考信息 [Lat Lng Alt]
            %         vn_ref - 地理系速度参考信息(m/s)
            %   输出：更新导航相关成员变量
            
            [phim, dvbm] = cnscl([wm vm], 1);       % 不可交换误差补偿
            obj.eth = earth(obj.avp(7:9),obj.avp(4:6));     % 地球参数更新
            [obj.sin_sig,obj.cos_sig] = getGridAngle(obj.avp(7:9));     % 计算格网角
            obj.CGn = [obj.cos_sig -obj.sin_sig 0; obj.sin_sig obj.cos_sig 0; 0 0 1];
            wie_G = obj.CGn*obj.eth.wnie;
            ss2 = obj.sin_sig^2;
            cs2 = obj.cos_sig^2;
            cLat = cos(obj.avp(7));
            sLng = sin(obj.avp(8));
            inv_Rx = ss2/obj.eth.RMh + cs2/obj.eth.RNh;
            inv_Ry = cs2/obj.eth.RMh + ss2/obj.eth.RNh;
            inv_tauf = (1/obj.eth.RMh - 1/obj.eth.RNh)*obj.sin_sig*obj.cos_sig;
            kG = (cLat*sLng)/sqrt(1-cLat^2*sLng^2);
            weG_G = [inv_tauf -inv_Ry; inv_Rx -inv_tauf; kG*inv_tauf -kG*inv_Ry]*obj.vG(1:2);
            
            % 速度更新
            if ~exist('vn_ref','var')       % 判断是否输入速度阻尼数据
                dotvG = qmulv(obj.qGb,dvbm) - cross((2.*wie_G+weG_G), obj.vG).*obj.nts +...
                        obj.eth.gn.*obj.nts;
                obj.vG = obj.vG + dotvG;
            else
                obj.vG = obj.CGn*vn_ref;
            end
            
            % 位置更新
            if ~exist('pos_ref','var')      % 判断是否输入位置阻尼数据
                obj = updateCGe(obj);
                dotRe = obj.CGe'*obj.vG;
                obj.pos = obj.pos + dotRe.*obj.nts;
            else
                obj.pos = blh2xyz(pos_ref);
            end

            % 姿态更新
            obj.qGb = qmul(rv2q(-(wie_G+weG_G).*obj.nts), qmul(obj.qGb, rv2q(phim)));
            
            obj = trans2avp(obj);       % 格网系导航结果转换成地理系结果输出
        end
        
        function obj = updateCGe(obj)
            % updateCeG 更新格网系到地球系的姿态矩阵
            sLat = sin(obj.avp(7));
            cLat = cos(obj.avp(7));
            sLng = sin(obj.avp(8));
            cLng = cos(obj.avp(8));
            s2Lat = sin(2*obj.avp(7));
            s2Lng = sin(2*obj.avp(8));
            sq = sqrt(1-cLat^2*sLng^2);
            obj.CGe = [-0.5*cLat^2*s2Lng/sq  sq     0.5*s2Lat*sLng/sq
                       -sLat/sq              0         cLat*cLng/sq
                       cLat*cLng         cLat*sLng        sLat         ];
        end
        
        function obj = trans2avp(obj)
            % trans2avp 格网系导航结果转换成地理系结果
            lla = xyz2blh(obj.pos);
            vn = obj.CGn'*obj.vG;
            [obj.sin_sig,obj.cos_sig] = getGridAngle(lla);
            obj.CGn = [obj.cos_sig -obj.sin_sig 0; obj.sin_sig obj.cos_sig 0; 0 0 1];
            att = q2att(qmul(m2qua(obj.CGn'),obj.qGb));
            obj.avp = [att; vn; lla];
        end
    end
end

function [sin_sig,cos_sig] = getGridAngle(lla)
    % getGridAngle 计算格网角
    % 输入: lla - [Lat Lng Alt] (rad rad m)
    % 输出: sin_sig - sin(sigma)
    %       cos_sig - cos(sigma)
    sLat = sin(lla(1));
    cLat = cos(lla(1));
    sLng = sin(lla(2));
    cLng = cos(lla(2));
    sin_sig = sLat*sLng/sqrt(1-cLat^2*sLng^2);
    cos_sig = cLng/sqrt(1-cLat^2*sLng^2);
end

